"""
Author: Morphlng
Date: 2024-03-19 14:57:39
LastEditTime: 2024-03-25 10:38:48
LastEditors: Morphlng
Description: HDMap test. Note that all these tests are based on Town01.xodr
FilePath: /DrivingGym/tests/common/hd_map/hd_map_test.py
"""

import math
import os

import carla

from driving_gym.simulation.common.geom import Location, Rotation, Transform
from driving_gym.simulation.common.hd_map import HDMap, LaneType, Waypoint

map_path = os.path.join(os.path.dirname(__file__), "Town01.xodr")
hd_map = HDMap(map_path)


def test_initialization():
    assert isinstance(hd_map._map, carla.Map), "Initialization failed"
    assert hd_map._grp, "Initialization failed"


def test_get_closest_waypoint():
    wpt1 = hd_map.get_closest_lane_waypoint(Location(113.69, 129.37, 0.2))
    assert isinstance(wpt1, Waypoint), "Get closest waypoint failed"
    assert wpt1.lane_type == LaneType.DRIVING, "Wrong lane type"

    wpt2 = hd_map.get_closest_lane_waypoint(Location(111.46, 125.03, 0.2))
    assert isinstance(wpt2, Waypoint), "Get closest waypoint failed"
    assert wpt2.lane_type == LaneType.SIDEWALK, "Wrong lane type"


def test_is_intersection():
    loc1 = Location(113.69, 129.37, 0.2)
    assert not hd_map.is_intersection(loc1), "Wrong intersection detection"

    loc2 = Location(97.56, 128.98, 0.2)
    assert hd_map.is_intersection(loc2), "Wrong intersection detection"


def test_is_on_lane():
    loc1 = Location(113.69, 129.37, 0.2)
    assert hd_map.is_on_lane(loc1), "Wrong lane detection"

    loc2 = Location(111.46, 125.03, 0.2)
    assert not hd_map.is_on_lane(loc2), "Wrong lane detection"


def test_are_on_same_lane():
    loc1 = Location(113.69, 129.37, 0.2)
    loc2 = Location(231.89, 129.07, 0.2)
    loc3 = Location(233.09, 133.7, 0.2)

    assert hd_map.are_on_same_lane(loc1, loc2), "Wrong lane detection"
    assert not hd_map.are_on_same_lane(loc1, loc3), "Wrong lane detection"


def test_is_on_opposite_lane():
    loc = Location(113.69, 129.37, 0.2)
    trans1 = Transform(loc, Rotation(0, 0, 0))
    trans2 = Transform(loc, Rotation(0, 180, 0))

    assert hd_map.is_on_opposite_lane(trans1), "Wrong opposite lane detection"
    assert not hd_map.is_on_opposite_lane(trans2), "Wrong opposite lane detection"


def test_is_at_stop():
    loc1 = Location(113.69, 129.37, 0.2)

    assert not hd_map.is_at_stop(loc1), "Wrong stop detection"


def test_distance_to_intersection():
    loc1 = Location(124.72, 129.44, 0.2)
    loc2 = Location(101.24, 129.15, 0.2)

    dist = loc1.distance(loc2)
    dist_to_intersection = hd_map.distance_to_intersection(loc1)
    assert math.isclose(
        dist_to_intersection, dist, abs_tol=1.0
    ), "Wrong distance to intersection"


def test_is_on_bidirectional_lane():
    loc1 = Location(113.69, 129.37, 0.2)
    assert not hd_map.is_on_bidirectional_lane(
        loc1
    ), "Wrong bidirectional lane detection"


def test_get_lane():
    loc1 = Location(113.69, 129.37, 0.2)
    lane = hd_map.get_lane(loc1)
    assert lane, "Get lane failed"


def test_get_left_lane():
    loc1 = Location(113.69, 129.37, 0.2)
    left_lane = hd_map.get_left_lane(loc1)
    assert left_lane, "Get left lane failed"
    assert left_lane.lane_id == -1, "Wrong left lane"

    loc2 = Location(175.03, 198.48, 0.2)
    left_lane2 = hd_map.get_left_lane(loc2)
    assert left_lane2, "Get left lane failed"
    assert left_lane2.lane_id == 1, "Wrong left lane"


def test_get_right_lane():
    loc1 = Location(339.43, 15.62, 0.2)
    right_lane = hd_map.get_right_lane(loc1)
    assert right_lane, "Get right lane failed"
    assert right_lane.lane_id == 2, "Wrong right lane"

    loc2 = Location(334.45, 15.24, 0.2)
    right_lane2 = hd_map.get_right_lane(loc2)
    assert right_lane2, "Get right lane failed"
    assert right_lane2.lane_id == -2, "Wrong right lane"


def test_get_all_lanes():
    loc1 = Location(113.69, 129.37, 0.2)
    lanes = hd_map.get_all_lanes(loc1)
    assert lanes, "Get all lanes failed"
    assert len(lanes) == 2, "Wrong number of lanes"


def test_compute_waypoints():
    start = Location(169.85, 59.46, 0.2)
    end = Location(338.68, 12.51, 0.2)

    plan = hd_map.compute_waypoints(start, end)
    assert plan, "Compute waypoints failed"
